/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/
#include "StdAfx.h"
#include "Rotation2.h"

namespace RobotPlant { namespace Math{
	Rotation2::Rotation2(void)
	{
		Cos = 1.0;
		Sin = 0.0;
	}

	Rotation2::Rotation2(const double angle_rad) : Sin(sin(angle_rad)),Cos(cos(angle_rad))
	{

	}

	Rotation2::Rotation2(const double ca,const double sa) : Sin(sa),Cos(ca)
	{

	}

	Rotation2::~Rotation2(void)
	{
	}

	double Rotation2::operator () (const int row,const int column) const
	{
		if((row < 0) || (row > 1))
			throw(new std::exception("Argument row index must be between 0 and 1."));
		if((column < 0) || (column > 1))
			throw(new std::exception("Argument column index must be between 0 and 1."));

		if (row==column) return Cos;
		if (row==0) 
			return Sin;
		else
			return -Sin;
	}

	Rotation2& Rotation2::operator = (const Rotation2& arg)
	{
		Cos = arg.Cos;
		Sin = arg.Sin;
		return *this;
	}

	bool operator == (const Rotation2& lhs, const Rotation2& rhs) 
	{
		return Equal(lhs,rhs);

	}

	bool operator != (const Rotation2& lhs, const Rotation2& rhs)
	{
		return !Equal(lhs,rhs);
	}

	Vector2 operator * (const Rotation2& lhs, const Vector2& rhs) 
	{
		return Vector2(rhs[0] * lhs.Cos - rhs[1] * lhs.Sin, rhs[0] * lhs.Sin + rhs[1] * lhs.Cos);
	}


	Rotation2 operator * (const Rotation2& lhs, const Rotation2& rhs)
	{
		return Rotation2( lhs.Cos * rhs.Cos - lhs.Sin * rhs.Sin, lhs.Sin * rhs.Cos + lhs.Cos * rhs.Sin);
	}

	void Rotation2::SetInverse() 
	{
		Sin=-Sin;
	}

	Rotation2 Rotation2::Inverse() const
	{
		return Rotation2(Cos,-Sin);
	}

	Vector2 Rotation2::Inverse(const Vector2& v) const 
	{
		return Vector2(v[0] * Cos + v[1] * Sin, -v[0] * Sin + v[1]*Cos);
	}

	void Rotation2::SetIdentity()
	{
		Cos = 1;
		Sin = 0;
	}

	Rotation2 Rotation2::Rot(const double angle)
	{
		return Rotation2(cos(angle),sin(angle));
	}

	void Rotation2::SetRot(const double angle) 
	{
		Cos = cos(angle);
		Sin = sin(angle);
	}

	double Rotation2::GetRot() const 
	{
		return atan2(Sin,Cos);
	}

	Rotation2 Rotation2::Identity() 
	{
		return Rotation2(1,0);
	}

	bool Equal(const Rotation2& a,const Rotation2& b,const double eps)
	{
		return ( Equal(a.Cos,b.Cos,eps) && Equal(a.Sin,b.Sin,eps) );
	}

	//Rotation2 AddDelta(const Rotation2& a,const Vector2& da,double dt)
	//{
	//	return a * Rotation2::Rot( a.Inverse( da ), dt );
	//}

	//Vector2 Diff(const Rotation2& R_a_b1,const Rotation2& R_a_b2,double dt)
	//{
	//	Rotation2 R_b1_b2( R_a_b1.Inverse() * R_a_b2 );
	//	return R_a_b1 * R_b1_b2.GetRot() / dt;
	//}

	void Random(Rotation2& R) 
	{
		double alfa;
		Random(alfa);
		R.SetRot(alfa);
	}

	void PosRandom(Rotation2& R) 
	{
		double alfa;
		Random(alfa);
		R.SetRot(alfa);
	}
}}
